#include "Graph_SearchAStar.h"
#include "IndexedPriorityQLow.h"
#include "Heuristic_Euclid.h"
#include "GraphNode.h"
#include <list>

template <class graph_type, class heuristic>
void Graph_SearchAStar<graph_type, heuristic>::Search()
{
	IndexedPriorityQLow<double> pq(m_FCosts, m_graph.NumNodes());

	pq.insert(m_iSource);

	while(!pq.empty()){

		int NextClosestNode = pq.Pop();

		m_ShortestPathTree[NextClosestNode] = m_ShearchFrontier[NextClosestNode];

		if(NextClosestNode == m_iTarget)
			return;

		graph_type::ConstEdgeIterator ConstEdgeItr(m_Graph, NextClosestNode);

		for(const Edge* pE = ConstEdgeItr.begin(); !ConstEdgeItr.End(); pE=ConstEdgeItr.next())
		{
			double HCost = heuristic::Calculate(m_Graph, m_iTarget, pE->getDestination());

			double GCost = m_GCosts[NextClosestNode] + pE->Cost();

			if(m_SearchFrontier[pE->getDestination()] == NULL)
			{
				m_FCosts[pE->getDestination()] = GCost + HCost;
				m_GCosts[pE->getDestination()] = GCost;

				pq.insert(pE->getDestination());

				m_SearchFrontier[pE->getDestination()] = pE;
			}
			else if((GCost < m_GCosts[pE->getDestination()]) &&
				(m_ShortestPathTree[pE->getDestination()]==NULL))
			{
				m_FCosts[pE->getDestination()] = GCost + HCost;
				m_GCosts[pE->getDestination()] = GCost;

				pq.ChangePriority(pE->getDestination());

				m_SearchFrontier[pE->getDestination()] = pE;
			}
		}
	}
}

template <class graph_type, class heuristic>
std::list<int> Graph_SearchAStar<graph_type, heuristic>::GetPathToTarget()const
{
  std::list<int> path;

  //just return an empty path if no target or no path found
  if (m_iTarget < 0)  return path;    

  int nd = m_iTarget;

  path.push_front(nd);
    
  while ((nd != m_iSource) && (m_ShortestPathTree[nd] != 0))
  {
    nd = m_ShortestPathTree[nd]->From();

    path.push_front(nd);
  }

  return path;
} 
